import os
import subprocess

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    # Run unitree lidar
    node1 = Node(
        package='unitree_lidar_ros2',
        executable='unitree_lidar_ros2_node',
        name='unitree_lidar_ros2_node',
        output='screen',
        parameters = [
            # 基本配置参数
            {'initialize_type': 1},              # 初始化类型，默认 1
            {'work_mode': 8},                    # 工作模式，默认 8
            {'use_system_timestamp': True},      # 使用系统时间戳，默认 True
            {'range_min': 0.0},                  # 最小范围，默认 0.0
            {'range_max': 50.0},                 # 最大范围，默认 50.0（代码中为 50.0，launch 中为 100.0，此处按代码调整）
            {'cloud_scan_num': 18},              # 点云扫描数，默认 18

            # 串口参数
            {'serial_port': '/dev/ttyACM0'},     # 串口路径，默认 "/dev/ttyACM0"
            {'baudrate': 4000000},               # 波特率，默认 4000000

            # UDP 参数
            {'lidar_port': 6101},                # 雷达端口，默认 6101
            {'lidar_ip': '192.168.1.2'},         # 雷达 IP，默认 "192.168.1.2"（launch 中为 "192.168.1.62"，按代码调整）
            {'local_port': 6201},                # 本地端口，默认 6201
            {'local_ip': '192.168.1.62'},        # 本地 IP，默认 "192.168.1.62"（launch 中为 "192.168.1.2"，按代码调整）

            # 坐标系和话题
            {'cloud_frame': 'unilidar_lidar'},   # 点云坐标系，默认 "unilidar_lidar"
            {'cloud_topic': 'unilidar/cloud'},   # 点云话题，默认 "unilidar/cloud"
            {'imu_frame': 'unilidar_imu'},       # IMU 坐标系，默认 "unilidar_imu"
            {'imu_topic': 'unilidar/imu'},       # IMU 话题，默认 "unilidar/imu"

            # 过滤参数
            {'filtered_cloud_z_min': -0.05},  # Z 轴最小高度，默认 -∞（代码中使用 -std::numeric_limits<double>::infinity()）
            {'filtered_cloud_z_max': 2.0},       # Z 轴最大高度，默认 1.5（launch 中为 1.0，按代码调整）
            {'obs_max_detection_range': 2.0},    # 最大检测距离，默认 2.0（launch 中为 1.0，按代码调整）
            {'obs_min_detection_range_xy': 0.05},# 最小检测距离（XY 平面），默认 0.05（launch 中为 0.10，按代码调整）
            {'obs_horizontal_fov_deg': 110.0},    # 水平视野角度，默认 90.0（launch 中为 60.0，按代码调整）


            # 新增可调参数
            {'buffer_size':5},                  # 缓冲区大小，默认 5

            # PCL 算法可调参数
            {'ground_segmentation_distance_threshold': 0.01}, # 地面分割距离阈值 (米)
            {'statistical_outlier_removal_mean_k': 30},       # 统计滤波邻域点数
            {'statistical_outlier_removal_stddev_thresh': 1.5}, # 统计滤波标准差阈值
            {'euclidean_cluster_tolerance': 0.15},            # 欧式聚类容差 (米)
            {'euclidean_cluster_min_size': 120},               # 聚类最小点数
            {'euclidean_cluster_max_size': 10000},            # 聚类最大点数

        ]

    )



# 2.1 odom -> unilidar_imu_initial
    static_tf_odom_to_imu = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='odom_to_imu_initial_tf_publisher',
        arguments=['0.0', '0.0', '0.22', '0.0', '0.0', '0.0', 'odom', 'unilidar_imu_initial'],
        output='screen'
    )

    # 2.2 unilidar_imu -> unilidar_lidar
    static_tf_imu_to_lidar = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='imu_to_lidar_tf_publisher',
        arguments=['0.007698', '0.014655', '-0.00667', '0.0', '0.0', '0.0', 'unilidar_imu', 'unilidar_lidar'],
        output='screen'
    )



    # # Run Rviz
    # package_path = subprocess.check_output(['ros2', 'pkg', 'prefix', 'unitree_lidar_ros2']).decode('utf-8').rstrip()
    # rviz_config_file = os.path.join(package_path, 'share', 'unitree_lidar_ros2', 'view.rviz')
    # print("rviz_config_file = " + rviz_config_file)
    # rviz_node = Node(
    #     package='rviz2',
    #     executable='rviz2',
    #     name='rviz2',
    #     arguments=['-d', rviz_config_file],
    #     output='log'
    # )
    # return LaunchDescription([node1, rviz_node])

    return LaunchDescription([
        node1,  # 先只启动雷达节点，跳过RViz
        static_tf_odom_to_imu,
        static_tf_imu_to_lidar,
        # rviz_node  # 如果需要启动RViz，取消此行注释
    ])